package com.rockwell.sniffyhunter.model;

import com.rockwell.sniffyhunter.utils.IRobotPositionDetector;
import com.rockwell.sniffyhunter.utils.IStateMachine;
import com.rockwell.sniffyhunter.utils.TurnerState;

/**
 * This class provides the robot for turning in the junction
 * The following actions will be performed:
 * <ul>
 * <li>Move into the junction.
 * <li>Turns the desired angle.
 * <li>Move out of the junction.
 * </ul>
 * <p>
 * 
 * @author      Sniffy Software Team
 * @version     1.0
 */

public class Turner extends StateObject {
	
	private static int MOVE_INDEX = 50; //mm
	private RobotCommandSequencer m_commandSequencer;
	private RobotCommand m_command;
	private RobotPosition m_robotPosition;
	private IRobotPositionDetector m_positionDetector;
	private ExitCode m_exitCode;
	boolean prevLeftwall2detected = false;
	boolean prevRightwall2detected = false;
	
	private enum ExitCode {
		NOT_STARTED,
		TURN_DONE,
		NO_WALL_AFTER_TURN
	}

	public Turner(Robot robot) {
		super(robot);
		m_commandSequencer = new RobotCommandSequencer(robot);
		m_command = new RobotCommand();
		m_positionDetector = robot.m_positionDetector;
		m_exitCode = ExitCode.NOT_STARTED;
	}
	
	public Turner(Robot robot, RobotCommand command) {
		super(robot);
		m_commandSequencer = new RobotCommandSequencer(robot);
		m_command = command;
		m_positionDetector = robot.m_positionDetector;
		m_exitCode = ExitCode.NOT_STARTED;
	}
	
	public ReturnResult PerformAction(RobotCommand command)  {
		
		if (machineState == TurnerState.START) {
			m_command = command;
			m_exitCode = ExitCode.NOT_STARTED;
			
			if (m_command.action == RobotAction.TurnBack)
				machineState = TurnerState.TURN;
		}

		return PerformAction();
	}
	
	@Override
	public void Initialize() {
		//m_commandSequencer.Initialize();
		machineState = TurnerState.START;
		machineRecoveryState = machineState;
	}
	
	@Override
	public boolean IsDone() {
		return machineState == TurnerState.DONE;
	}
	
	public boolean isNoWallAfterTurn() {
		return (m_exitCode == ExitCode.NO_WALL_AFTER_TURN);
	}

	public IStateMachine jogIntoJunction() {

		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.Jog, 4);

		if (returnVal == ReturnResult.Completed) {
			log("moveIntoJunction.Turner", 4);
			machineState = TurnerState.CHECK_JOG_INTO_JUNCTION_DONE;
		}
		
		return machineState;
	}
	
	public IStateMachine checkJogIntoJunctionDone() {

		m_robotPosition = m_positionDetector.getPosition();

		if ((!m_robotPosition.leftwall2detected && !m_robotPosition.wheelMoving) || 
				(!m_robotPosition.rightwall2detected  && !m_robotPosition.wheelMoving) || 
				m_robotPosition.frontwalldetected && !m_robotPosition.wheelMoving) {
				log("isInJuntion = true.Turner", 4);
				machineState = TurnerState.STOP_JOG_INTO_JUNCTION;
			}

		return machineState;
	}
	
	public IStateMachine stopJogIntoJunction() {

		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.Stop);

		if (returnVal == ReturnResult.Completed) {
			log("stopJogIntoJunction.Turner", 4);
			
			m_robotPosition = m_positionDetector.getPosition();
			
			if (m_robotPosition.frontwalldetected) {
				log("frontwalldetected.Turner", 4);
				machineState = TurnerState.TURN;
			}
			else {
				log("frontwall not detected.Turner", 4);
				machineState = TurnerState.MOVE_INTO_JUNCTION;
			}
		}
		
		return machineState;
	}
	
	public IStateMachine moveIntoJunction() {

		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.MoveForward2, MOVE_INDEX);

		if (returnVal == ReturnResult.Completed) {
			log("moveIntoJunction.Turner", 4);
			machineState = TurnerState.TURN;
		}
		
		return machineState;
	}
	
	public IStateMachine turn() {

		ReturnResult returnVal = m_commandSequencer.PerformAction(m_command.action, m_command.param1);

		if (returnVal == ReturnResult.Completed) {
			log("turn.Turner", 4);
			
			if (m_command.action == RobotAction.TurnBack) {
				m_exitCode = ExitCode.TURN_DONE;
				machineState = TurnerState.DONE;
			}
			else {
				m_robotPosition = m_positionDetector.getPosition();
				prevLeftwall2detected = m_robotPosition.leftwall2detected;
				prevRightwall2detected = m_robotPosition.rightwall2detected;
				machineState = TurnerState.JOG_OUT_OF_JUNCTION;
			}
		}
		
		return machineState;
	}
	
	public IStateMachine jogOutOfJunction() {

		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.Jog, 5);

		if (returnVal == ReturnResult.Completed) {
			log("moveOutOfJunction.Turner", 4);
			machineState = TurnerState.CHECK_JOG_OUT_OF_JUNCTION_DONE;
		}
		
		return machineState;
	}
	
	public IStateMachine checkJogOutOfJunctionDone() {

		m_robotPosition = m_positionDetector.getPosition();

		if ((!prevLeftwall2detected && m_robotPosition.leftwall2detected && !m_robotPosition.wheelMoving)   ||
			(!prevRightwall2detected && m_robotPosition.rightwall2detected && !m_robotPosition.wheelMoving) ||
			m_robotPosition.frontwalldetected && !m_robotPosition.wheelMoving) {
			log("isInJuntion = true.Turner", 4);
			machineState = TurnerState.STOP_JOG_OUT_OF_JUNCTION;
		}
		
		return machineState;
	}
	
	public IStateMachine stopJogOutOfJunction() {

		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.Stop);

		if (returnVal == ReturnResult.Completed) {
			log("stopJogIntoJunction.Turner", 4);
			machineState = TurnerState.MOVE_OUT_OF_JUNCTION;
		}
		
		return machineState;
	}
	
	public IStateMachine moveOutOfJunction() {

		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.Move2, MOVE_INDEX);

		if (returnVal == ReturnResult.Completed) {
			log("moveOutOfJunction.Turner", 4);
			machineState = TurnerState.CHECK_MOVE_OUT_OF_JUNCTION_DONE;
		}
		
		return machineState;
	}
	
	public IStateMachine checkMoveOutOfJunctionDone() {

		m_robotPosition = m_positionDetector.getPosition();
		
		if (!m_robotPosition.leftwalldetected && !m_robotPosition.rightwalldetected) {
			log("exit detected.Turner", 4);
			m_exitCode = ExitCode.NO_WALL_AFTER_TURN;
			machineState = TurnerState.DONE;
		}
		else {
			log("isOutOfJuntion = true.Turner", 4);
			m_exitCode = ExitCode.TURN_DONE;
			machineState = TurnerState.DONE;
		}
		
		return machineState;
	}
}